/*-----------------------------------------------------------------------*/
/* Low level disk I/O module skeleton for Petit FatFs (C)ChaN, 2014      */
/*-----------------------------------------------------------------------*/

#include "diskio.h"
#include "spi.h"

#define CMD0        0
#define CMD1        1
#define CMD8        8
#define CMD16       16
#define CMD17       17
#define CMD24       24
#define ACMD41      41
#define CMD55       55
#define CMD58       58

#define T250MS      6250
#define T100MS      2500

BYTE card_type;
WORD byte_cnt;

/*-----------------------------------------------------------------------*/
/* Send command to SD                                                    */
/*-----------------------------------------------------------------------*/

BYTE send_cmd(BYTE cmd, DWORD arg, BYTE crc) {
    BYTE res, i = 16;
    
    spi_send(cmd | 0x40);                           // send command
    spi_send((BYTE) (arg >> 24));                   // send argument
    spi_send((BYTE) (arg >> 16));
    spi_send((BYTE) (arg >> 8));
    spi_send((BYTE) (arg));
    
    spi_send(crc | 0x01);                           // send CRC
    
    while (((res = spi_receive()) == 0xFF) && i--); // read R1
    
    return res;
}

/*-----------------------------------------------------------------------*/
/* Initialize Disk Drive                                                 */
/*-----------------------------------------------------------------------*/

DSTATUS disk_initialize (void) {
    BYTE i = 0;
    BYTE res[4];
    
    spi_init();
    
    spi_deselect();
    for (i = 0; i < 10; i++) {
        spi_receive();
    }
    
    spi_select();
    if (send_cmd(CMD0, 0x0, 0x94) == 0x1) {                     // enter Idle state
        if (send_cmd(CMD8, 0x1AA, 0x86) == 0x1) {               // SDv2
            for (i = 0; i < 4; i++) res[i] = spi_receive();     // read R7 (OCR)
            i = 100;
            do {                                                // wait until exit Idle state
                send_cmd(CMD55, 0x0, 0x0);
            } while (send_cmd(ACMD41, 0x40000000, 0x0) && --i);
            if (!send_cmd(CMD58, 0x0, 0x0) && i) {
                for (i = 0; i < 4; i++) res[i] = spi_receive(); // read R7 (OCR)
                card_type = (res[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2;
            }
        } else {                                                // SDv1 or MMCv3
            for (i = 0; i < 4; i++) res[i] = spi_receive();     // read R7 (OCR)
            send_cmd(CMD55, 0x0, 0x0);
            if (send_cmd(ACMD41, 0x0, 0x0) <= 1) {              // SDv1
                card_type = CT_SD1;
                i = 100;
                do {                                            // wait until exit Idle state
                    send_cmd(CMD55, 0x0, 0x0);
                } while (send_cmd(ACMD41, 0x0, 0x0) && --i);
            } else {                                            // MMCv3
                card_type = CT_MMC;
                
                i = 100;
                while (send_cmd(CMD1, 0x0, 0x0) && --i);        // wait until exit Idle state
            }
            
            if (!send_cmd(CMD16, 512, 0x0) || !i) {
                card_type = 0;
            }
        }
    }
    
    spi_deselect();
    
    return (card_type ? 0 : STA_NOINIT);
}



/*-----------------------------------------------------------------------*/
/* Read Partial Sector                                                   */
/*-----------------------------------------------------------------------*/

DRESULT disk_readp (BYTE* buff, DWORD sector, UINT offset, UINT count) {
	if (offset + count > 512) {
        return RES_PARERR;
    }
    if (!card_type) {
        return RES_NOTRDY;
    }
    
    BYTE res, read;
    WORD i;
    
    spi_select();
    
    res = send_cmd(CMD17, (card_type & CT_BLOCK ? sector : sector * 512), 0x0);
    
    if(res != 0xFF) {
        // wait for a response
        for (i = 0; i < T100MS; i++) {
            if ((read = spi_receive()) != 0xFF) {
                break;
            }
        }
        
        // if response is 0xFE
        if(read == 0xFE) {
            // skip offset
            for (i = 0; i < offset; i++) {
                spi_receive();
            }
            
            // read data
            for (i = 0; i < count; i++) {
                buff[i] = spi_receive();
            }
            
            // skip remaining bytes
            for (i = 0; i < 512 - count - offset; i++) {
                spi_receive();
            }
            
            // read CRC
            spi_receive();
            spi_receive();
        }
    }
    
    spi_deselect();
    
    return RES_OK;
}



/*-----------------------------------------------------------------------*/
/* Write Partial Sector                                                  */
/*-----------------------------------------------------------------------*/

#if PF_USE_WRITE
DRESULT disk_writep (
	const BYTE* buff,		/* Pointer to the data to be written, NULL:Initiate/Finalize write operation */
	DWORD sc		        /* Sector number (LBA) or Number of bytes to send */
)
{
	DRESULT res = RES_ERROR;
	WORD i;
	BYTE read;

	if (!buff) {
		if (sc) {
			// Initiate write process
		    spi_select();
		    if (send_cmd(CMD24, (card_type & CT_BLOCK ? sc : sc * 512), 0x0) != 0x0) {
		        return RES_NOTRDY;
		    }
		    spi_send(0xFE);
		    byte_cnt = 512;
		    return RES_OK;
		} else {
			// Finalize write process
		    while (byte_cnt--) {
		        spi_send(0x0);
		    }
		    for (i = 0; i < T250MS; i++) {
		        if ((read = spi_receive()) != 0xFF) {
		            break;
		        }
		    }
		    if ((read & 0x1F) == 0x05) {
		        i = T250MS;
		        while (--i) {
		            if (spi_receive() != 0x0) {
		                break;
		            }
		        }
		        if (i) {
		            res = RES_OK;
		        }
		    }
		    spi_deselect();
		}
	} else {
		// Send data to the disk
	    if (sc > byte_cnt) {
	        return RES_PARERR;
	    }
	    byte_cnt -= sc;
	    for (i = 0; i < sc; i++) {
	        spi_send(buff[i]);
	    }
	    res = RES_OK;
	}

	return res;
}
#endif
